#include "StepperDriver2PWM.h"

StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _in1a, int _in1b, int _pwm2, int _in2a, int _in2b, int en1, int en2)
{
    // Pin initialization
    pwm1 = _pwm1; // phase 1 pwm pin number
    dir1a = _in1a; // phase 1 INA pin number
    dir1b = _in1b; // phase 1 INB pin number
    pwm2 = _pwm2; // phase 2 pwm pin number
    dir2a = _in2a; // phase 2 INA pin number
    dir2b = _in2b; // phase 2 INB pin number

    // enable_pin pin
    enable_pin1 = en1;
    enable_pin2 = en2;

    // default power-supply value
    voltage_power_supply = DEF_POWER_SUPPLY;
    voltage_limit = NOT_SET;

}

StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, int en1, int en2)
{
    // Pin initialization
    pwm1 = _pwm1; // phase 1 pwm pin number
    dir1a = _dir1; // phase 1 direction pin
    pwm2 = _pwm2; // phase 2 pwm pin number
    dir2a = _dir2; // phase 2 direction pin
    // these pins are not used
    dir1b = NOT_SET;
    dir2b = NOT_SET;

    // enable_pin pin
    enable_pin1 = en1;
    enable_pin2 = en2;

    // default power-supply value
    voltage_power_supply = DEF_POWER_SUPPLY;
    voltage_limit = NOT_SET;

}

// enable motor driver
void StepperDriver2PWM::enable()
{
    // enable_pin the driver - if enable_pin pin available
    if (_isset(enable_pin1)) digitalWrite(enable_pin1, HIGH);
    if (_isset(enable_pin2)) digitalWrite(enable_pin2, HIGH);
    // set zero to PWM
    setPwm(0, 0);
}

// disable motor driver
void StepperDriver2PWM::disable()
{
    // set zero to PWM
    setPwm(0, 0);
    // disable the driver - if enable_pin pin available
    if (_isset(enable_pin1)) digitalWrite(enable_pin1, LOW);
    if (_isset(enable_pin2)) digitalWrite(enable_pin2, LOW);

}

// init hardware pins   
int StepperDriver2PWM::init()
{
    // PWM pins
    pinMode(pwm1, OUTPUT);
    pinMode(pwm2, OUTPUT);
    pinMode(dir1a, OUTPUT);
    pinMode(dir2a, OUTPUT);
    if (_isset(dir1b)) pinMode(dir1b, OUTPUT);
    if (_isset(dir2b)) pinMode(dir2b, OUTPUT);

    if (_isset(enable_pin1)) pinMode(enable_pin1, OUTPUT);
    if (_isset(enable_pin2)) pinMode(enable_pin2, OUTPUT);

    // sanity check for the voltage limit configuration
    if (!_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply;

    // Set the pwm frequency to the pins
    // hardware specific function - depending on driver and mcu
    _configure2PWM(pwm_frequency, pwm1, pwm2);
    return 0;
}


// Set voltage to the pwm pin
void StepperDriver2PWM::setPwm(float Ua, float Ub)
{
    float duty_cycle1(0.0), duty_cycle2(0.0);
    // limit the voltage in driver
    Ua = _constrain(Ua, -voltage_limit, voltage_limit);
    Ub = _constrain(Ub, -voltage_limit, voltage_limit);
    // hardware specific writing
    duty_cycle1 = _constrain(abs(Ua) / voltage_power_supply, 0.0, 1.0);
    duty_cycle2 = _constrain(abs(Ub) / voltage_power_supply, 0.0, 1.0);

    // phase 1 direction
    digitalWrite(dir1a, Ua >= 0 ? LOW : HIGH);
    if (_isset(dir1b)) digitalWrite(dir1b, Ua <= 0 ? LOW : HIGH);
    // phase 2 direction
    digitalWrite(dir2a, Ub >= 0 ? LOW : HIGH);
    if (_isset(dir2b)) digitalWrite(dir2b, Ub <= 0 ? LOW : HIGH);

    // write to hardware
    _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2);
}